﻿using System;
using System.Collections.Generic;
using System.Text;

using Microsoft.Xna.Framework;

namespace Daybreak.Core.SceneGraph
{
    /// <summary>
    /// A Transform is a group node for which all children are transformed by a 4x4 matrix. 
    /// </summary>
    public abstract class Transform : Group
    {
        /// <summary>
        /// Transform.Matrix44 - is a subclass of Transform which has an Matrix 
        /// which represents a 4x4 transformation of its children from local coordinates 
        /// into the Transform's parent coordinates. 
        /// </summary>
        public class Matrix44 : Transform
        {
            public Matrix LocalMatrix;

            public Matrix44()
            {
                this.LocalMatrix = Matrix.Identity;
            }

            public Matrix44(Matrix mat)
            {
                this.LocalMatrix = mat;
            }

            public override void Accept(Streamer streamer)
            {
                //TODO ..
            }

            public override void Inverse(out Matrix result)
            {
                Matrix.Invert(ref LocalMatrix, out result);
            }

            public override void LocalToWorld(ref Matrix result)
            {
                // pre mult
                //result = result * Matrix;
                //Matrix.Multiply(ref result, ref Matrix, out result);
                Matrix.Multiply(ref LocalMatrix, ref result, out result);
            }

            public override void WorldToLocal(ref Matrix result)
            {
                Matrix inv;

                Inverse(out inv);

                // post mult
                //result = inv * result;
                //Matrix.Multiply(ref inv, ref result, out result);
                Matrix.Multiply(ref result, ref inv, out result);

            }

            public void Move(float x, float y, float z, bool increment)
            {
                if (increment)
                {
                    Vector3 pos = LocalMatrix.Translation;
                    pos.X += x;
                    pos.Y += y;
                    pos.Z += z;
                    LocalMatrix.Translation = pos;
                }
                else
                    LocalMatrix.Translation = new Vector3(x, y, z);
            }

            public void Rotate(ref Matrix rot, bool increment)
            {
                if (increment)
                    Matrix.Multiply(ref LocalMatrix, ref rot, out LocalMatrix);
                else
                    CopyMatrixRotation(ref rot, ref LocalMatrix);
            }

            public void RotateX(float angle, bool increment)
            {
                Matrix rot;
                Matrix.CreateRotationX(MathHelper.ToRadians(angle), out rot);

                Rotate(ref rot, increment);
            }

            public void RotateY(float angle, bool increment)
            {
                Matrix rot;
                Matrix.CreateRotationY(MathHelper.ToRadians(angle), out rot);

                Rotate(ref rot, increment);
            }

            public void RotateZ(float angle, bool increment)
            {
                Matrix rot;
                Matrix.CreateRotationZ(MathHelper.ToRadians(angle), out rot);

                Rotate(ref rot, increment);
            }

            private void CopyMatrixRotation(ref Matrix from, ref Matrix to)
            {
                to.M11 = from.M11;
                to.M12 = from.M12;
                to.M13 = from.M13;
                to.M21 = from.M21;
                to.M22 = from.M22;
                to.M23 = from.M23;
                to.M31 = from.M31;
                to.M32 = from.M32;
                to.M33 = from.M33;

            }

            public override void SetTransformation(ref Matrix matrix)
            {
                LocalMatrix = matrix;
            }

            public override void GetTransformation(out Matrix matrix)
            {
                matrix = LocalMatrix;
            }

        }

        /// <summary>
        /// Dof taransform class encapsulates Multigen DOF behavior. 
        /// </summary>
        public class DofNode : Transform
        {
            public class Constraint
            {
                public Vector3 MinRotationAngle;
                public Vector3 MaxRotationAngle;

                // current state (0 - at the minimum; 1 - at the maximum)
                private float _curState;
                private float _targetState;
                private bool _loop;
                private float _sps;

                public Constraint(bool loop)
                {
                    _loop = loop;
                }

                /// <summary>
                /// Sets target state for this constraint
                /// </summary>
                /// <param name="state">target state (0..1)</param>
                /// <param name="dps">speed of advance (degrees per second)</param>
                internal void AdvanceToState(float state, float dps)
                {
                    _targetState = state;

                    float totalAngle = TotalAngle();

                    _sps = dps / totalAngle;

                    if (_curState > _targetState)
                        _sps = -_sps;

                }

                public void Update(Transform.DofNode parent, float delta)
                {
                    if (_curState == _targetState)
                        return;

                    Vector3 curAngles = Vector3.Zero;

                    Vector3.Lerp(ref MinRotationAngle, ref MaxRotationAngle, _curState, out curAngles);

                    float stateDelta = _sps * delta;

                    _curState += stateDelta;

                    if (_sps > 0 && _curState >= _targetState)
                    {
                        _curState = _targetState;

                        Vector3.Lerp(ref MinRotationAngle, ref MaxRotationAngle, _curState, out curAngles);

                        if (_loop)
                            _curState = 0;
                    }

                    if (_sps < 0 && _curState <= _targetState)
                    {
                        _curState = _targetState;

                        Vector3.Lerp(ref MinRotationAngle, ref MaxRotationAngle, _curState, out curAngles);

                        if (_loop)
                            _curState = 1;

                    }

                    parent.YawAngle = curAngles.Y;
                    parent.PitchAngle = curAngles.X;
                    parent.RollAngle = curAngles.Z;

                }

                public void SetState(float state)
                {
                    _curState = state;
                }

                public float TotalAngle()
                {
                    Vector3 deltaVec = MaxRotationAngle - MinRotationAngle;

                    return Math.Abs(deltaVec.Length());
                }

                public override string ToString()
                {
                    return string.Format("Constraint from:{0} to:{1} (total:{2})", MinRotationAngle, MaxRotationAngle, TotalAngle());
                }

            }

            public class ConstraintGroup
            {
                public string Name;

                public List<DofNode> DofNodeList;

                public ConstraintGroup(string name)
                {
                    Name = name;
                    DofNodeList = new List<DofNode>();
                }

                /// <summary>
                /// Sets target state for this list of dof node constraints
                /// </summary>
                /// <param name="state">target state (0..1) 0=start,1=end</param>
                /// <param name="dps">speed of advance (degrees per second)</param>
                public void AdvanceToState(float state, float dps)
                {
                    for (int i = 0; i < DofNodeList.Count; i++)
                        DofNodeList[i].AdvanceConstraintToState(state, dps);
                }

                public override string ToString()
                {
                    return string.Format("ConstraintGroup:[{0}] nodes:{1}", Name, DofNodeList.Count);
                }
            }

            bool _isDirty;

            float _y_yawAngle;
            float _x_pitchAngle;
            float _z_rollAngle;
            Vector3 _translation;
            Matrix _cachedMatrix;
            Constraint _constraint;

            public DofNode()
            {
                _isDirty = true;
            }

            public override void Accept(Streamer streamer)
            {
                //TODO ..
            }

            public Constraint SetConstraint(Quaternion start, Quaternion end)
            {
                _constraint = new Constraint(false);

                Matrix matrix;

                Matrix.CreateFromQuaternion(ref start, out matrix);

                DofNode.MatrixToEulerDegrees(ref matrix, out _constraint.MinRotationAngle);

                Matrix.CreateFromQuaternion(ref end, out matrix);

                DofNode.MatrixToEulerDegrees(ref matrix, out _constraint.MaxRotationAngle);

                return _constraint;

            }


            public override void Update(NodeVisitor updater, float deltaTime)
            {
                base.Update(updater, deltaTime);

                if (_constraint != null)
                    _constraint.Update(this, deltaTime);
            }

            /// <summary>
            /// Sets target state for this dof node constraint
            /// </summary>
            /// <param name="state">target state (0..1) 0=start,1=end</param>
            /// <param name="dps">speed of advance (degrees per second)</param>
            public void AdvanceConstraintToState(float state, float dps)
            {
                if (_constraint != null)
                    _constraint.AdvanceToState(state, dps);
            }


            public override void Inverse(out Matrix result)
            {
                if (_isDirty)
                    UpdateMatrix();

                Matrix.Invert(ref _cachedMatrix, out result);
            }

            public override void LocalToWorld(ref Matrix result)
            {
                if (_isDirty)
                    UpdateMatrix();

                // pre mult
                //result = result * Matrix;
                //Matrix.Multiply(ref result, ref _cachedMatrix, out result);
                Matrix.Multiply(ref _cachedMatrix, ref result, out result);
            }

            public override void WorldToLocal(ref Matrix result)
            {
                Matrix inv;

                Inverse(out inv);

                // post mult
                //result = inv * result;
                //Matrix.Multiply(ref inv, ref result, out result);
                Matrix.Multiply(ref result, ref _cachedMatrix, out result);

            }

            public override void GetTransformation(out Matrix matrix)
            {
                if (_isDirty)
                    UpdateMatrix();

                matrix = _cachedMatrix;
            }

            public override void SetTransformation(ref Matrix matrix)
            {
                _translation = matrix.Translation;

                SetEulerFromMatrix(ref matrix);

            }

            /// <summary>Sets XYZ Euler angles decomposition.</summary>
            public void SetEulerFromMatrix(ref Matrix mat)
            {
                _isDirty = true;

                MatrixToEuler(ref mat, out _y_yawAngle, out _x_pitchAngle, out _z_rollAngle);

            }

            /// <summary>Gets XYZ Euler angles decomposition (degrees).</summary>
            public static void MatrixToEulerDegrees(ref Matrix mat, out Vector3 v)
            {
                MatrixToEulerDegrees(ref mat, out v.Y, out v.X, out v.Z);
            }

            /// <summary>Gets XYZ Euler angles decomposition (degrees).</summary>
            public static void MatrixToEulerDegrees(ref Matrix mat, out float y_yaw, out float x_pitch, out float z_roll)
            {
                MatrixToEuler(ref mat, out y_yaw, out x_pitch, out z_roll);

                y_yaw = MathHelper.ToDegrees(y_yaw);
                x_pitch = MathHelper.ToDegrees(x_pitch);
                z_roll = MathHelper.ToDegrees(z_roll);
            }

            /// <summary>Gets XYZ Euler angles decomposition.</summary>
            public static void MatrixToEuler(ref Matrix mat, out float y_yaw, out float x_pitch, out float z_roll)
            {
                if (mat.M13 < (1f - float.Epsilon))
                {
                    if (mat.M13 > (-1f + float.Epsilon))
                    {
                        z_roll = (float)Math.Atan2(mat.M12, mat.M11);
                        y_yaw = (float)Math.Asin(-mat.M13);
                        x_pitch = (float)Math.Atan2(mat.M23, mat.M33);

                        return;
                    }

                    z_roll = -((float)Math.Atan2(mat.M21, mat.M31));
                    y_yaw = 0.5f * (float)Math.PI;
                    x_pitch = 0f;

                    return;
                }

                z_roll = (float)Math.Atan2(-mat.M21, -mat.M31);
                y_yaw = -0.5f * (float)Math.PI;
                x_pitch = 0f;

            }

            public void Move(int x, int y, int z, bool increment)
            {
                if (increment)
                {
                    _translation.X += x;
                    _translation.Y += y;
                    _translation.Z += z;
                }
                else
                {
                    _translation.X = x;
                    _translation.Y = y;
                    _translation.Z = z;
                }

                _isDirty = true;

            }

            private void UpdateMatrix()
            {
                //Matrix trans;
                //Matrix.CreateTranslation(ref _translation, out trans);

                Matrix.CreateFromYawPitchRoll(_y_yawAngle, _x_pitchAngle, _z_rollAngle, out _cachedMatrix);

                _cachedMatrix.Translation = _translation;
                //Matrix.Multiply(ref _cachedMatrix, ref trans, out _cachedMatrix);

                _isDirty = false;
            }

            /// <summary>
            /// Angle of rotation, in degrees, around the y-axis.
            /// </summary>
            public float YawAngle
            {
                get { return MathHelper.ToDegrees(_y_yawAngle); }
                set
                {
                    _y_yawAngle = MathHelper.ToRadians(value);
                    _isDirty = true;
                }
            }
            /// <summary>
            /// Angle of rotation, in degrees, around the x-axis.
            /// </summary>
            public float PitchAngle
            {
                get { return MathHelper.ToDegrees(_x_pitchAngle); }
                set
                {
                    _x_pitchAngle = MathHelper.ToRadians(value);
                    _isDirty = true;
                }
            }
            /// <summary>
            /// Angle of rotation, in degrees, around the z-axis.
            /// </summary>
            public float RollAngle
            {
                get { return MathHelper.ToDegrees(_z_rollAngle); }
                set
                {
                    _z_rollAngle = MathHelper.ToRadians(value);
                    _isDirty = true;
                }
            }

            public Vector3 Translation
            {
                get { return _translation; }
                set
                {
                    _translation = value;
                    _isDirty = true;
                }
            }

            public bool HasConstraints
            {
                get { return _constraint != null; }
            }

        }

        public Transform() { }


        public override void Accept(NodeVisitor visitor)
        {
            visitor.VisitTransform(this);
        }

        public virtual void Inverse(out Matrix result)
        {
            result = Matrix.Identity;
        }


        /// <summary>
        /// Sets this transform node as XNA matrix
        /// </summary>
        public abstract void SetTransformation(ref Matrix matrix);
        /// <summary>
        /// Gets this transform node as XNA matrix
        /// </summary>
        public abstract void GetTransformation(out Matrix matrix);

        public override string ToString()
        {
            return string.Format("[Transform:{0}] {1} {2}", GetType().Name, Name, base.ToString());
        }

        public static void QuaternionToAngleAxis(ref Quaternion quat, ref float angle, ref Vector3 axis)
        {
            // The quaternion representing the rotation is
            //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
            float x = quat.X;
            float y = quat.Y;
            float z = quat.Z;

            float sqrLength = x * x + y * y + z * z;

            if (sqrLength > 0.0f)
            {
                angle = 2.0f * (float)Math.Acos(quat.W);
                float invLength = 1.0f / (float)Math.Sqrt(sqrLength);
                axis.X = x * invLength;
                axis.Y = y * invLength;
                axis.Z = z * invLength;
            }
            else
            {
                angle = 0.0f;
                axis.X = 1.0f;
                axis.Y = 0.0f;
                axis.Z = 0.0f;
            }
        }

        public static void BBoxCenter(ref BoundingBox bbox, out Vector3 center)
        {
            Vector3.Add(ref bbox.Min, ref bbox.Max, out center);
            Vector3.Multiply(ref center, 0.5f, out center);
        }

        public static void TransformBox(ref BoundingBox inBox, ref Matrix mat, out BoundingBox result)
        {
            Vector3 position = (Vector3)(0.5f * (inBox.Min + inBox.Max));
            Vector3 offset;
            Vector3 dim;

            Vector3.Subtract(ref inBox.Max, ref position, out offset);

            dim.X = ((offset.X * Math.Abs(mat.M11)) + (offset.Y * Math.Abs(mat.M21))) + (offset.Z * Math.Abs(mat.M31));
            dim.Y = ((offset.X * Math.Abs(mat.M12)) + (offset.Y * Math.Abs(mat.M22))) + (offset.Z * Math.Abs(mat.M32));
            dim.Z = ((offset.X * Math.Abs(mat.M13)) + (offset.Y * Math.Abs(mat.M23))) + (offset.Z * Math.Abs(mat.M33));

            Vector3.Transform(ref position, ref mat, out position);

            Vector3.Subtract(ref position, ref dim, out result.Min);
            Vector3.Add(ref position, ref dim, out result.Max);

        }

    }

}
